///*
// * Robot2012 Class
// * Team 675 - TJ Dooley
// * Created January 2012
// * 
// * Center piece of the 2012 Robotics Code.
// * 
// */

#include "Awaiting Review/Include.h"
#include "Awaiting Review/AutonomousHandler.h"
#include "Awaiting Review/RobotDriveCustom.h"
#include "Awaiting Review/RobotPayload.h"

class Robot2012 : public SimpleRobot
{
	Team675::TankDrive m_robotDriveType;
	Team675::RobotDrive m_robotDrivetrain;
	Team675::RobotPayload m_robotPayload;
	Team675::AutonomousHandler m_robotAutonomous;
public:
	Robot2012() : m_robotDrivetrain(&m_robotDriveType),
		m_robotPayload(Team675::RobotPayload::CONTROL_DOUBLE_JOYSTICK, 
			Team675::RobotPayload::TRACKING_AUTOMATIC_CAMERA, 
			Team675::RobotPayload::SHOOTING_MANUAL),
		m_robotAutonomous(Team675::AutonomousHandler::AUTONOMOUS_STILL, &m_robotDrivetrain, &m_robotPayload)
	{
		printf("ROBOT 2012\n");
		
//		m_robotAutonomous.InitChooser();
	}

	void Autonomous()
	{
		while(DriverStation::GetInstance()->IsAutonomous())
		{
			m_robotAutonomous.Update();
			
			Wait(0.005);
		}
	}

	void OperatorControl()
	{
		m_robotDrivetrain.SetDrive(&m_robotDriveType);
		m_robotPayload.SetControlMode(Team675::RobotPayload::CONTROL_DOUBLE_JOYSTICK);
		m_robotPayload.SetTrackingMode(Team675::RobotPayload::TRACKING_AUTOMATIC_CAMERA);
		m_robotPayload.SetShootingMode(Team675::RobotPayload::SHOOTING_MANUAL);
		
		while(DriverStation::GetInstance()->IsOperatorControl())
		{
			m_robotDrivetrain.Update();
			m_robotPayload.Update();
			
			Wait(0.005);
		}
	}
};

START_ROBOT_CLASS(Robot2012);

